#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <mosquitto.h>
#include <jsoncpp/json/json.h>
#include <string>
#include <cstring>
#include "mqtt_bridge/json_data.hpp"

// rclcpp::Node代表该类是ros2节点
class SeatMqttBridge : public rclcpp::Node
{
public:
    // 节点名称是seat_mqtt_bridge
    SeatMqttBridge() : Node("seat_mqtt_bridge"), is_connected_(false), stop_connect_thread_(false)
    {
        ros2_send  = this->declare_parameter<int>("ros2_send", 0);
        mqtt_host_ = this->declare_parameter<std::string>("mqtt_net.host", "localhost");
        mqtt_port_ = this->declare_parameter<int>("mqtt_net.port", 1883);

        //从mqtt发送到ros
        //从mqtt broker发来的
        command_mqtt_topic_ = declare_parameter<std::string>("mqtt_to_ros.command.mqtt", "/fangshe/robot/control/command");
        //发送到ros网络
        command_ros_topic_ = declare_parameter<std::string>("mqtt_to_ros.command.ros", "/fangshe/robot/control/command");

        stop_mqtt_topic_ = declare_parameter<std::string>("mqtt_to_ros.stop.mqtt", "/fangshe/robot/control/stop");
        stop_ros_topic_ = declare_parameter<std::string>("mqtt_to_ros.stop.ros", "/fangshe/robot/control/stop");


        //从ros发送到mqtt
        system_ros_topic_ = this->declare_parameter<std::string>("ros_to_mqtt.system.ros", "/fangshe/robot/status/system");
        system_mqtt_topic_ = this->declare_parameter<std::string>("ros_to_mqtt.system.mqtt", "/fangshe/robot/status/system");

        pose_ros_topic_ = this->declare_parameter<std::string>("ros_to_mqtt.pose.ros", "/fangshe/robot/status/pose");
        pose_mqtt_topic_ = this->declare_parameter<std::string>("ros_to_mqtt.pose.mqtt", "/fangshe/robot/status/pose");

        task_ros_topic_ = this->declare_parameter<std::string>("ros_to_mqtt.task.ros", "/fangshe/robot/status/task");
        task_mqtt_topic_ = this->declare_parameter<std::string>("ros_to_mqtt.task.mqtt", "/fangshe/robot/status/task");

        gripper_ros_topic_ = this->declare_parameter<std::string>("ros_to_mqtt.gripper.ros", "/fangshe/robot/status/gripper");
        gripper_mqtt_topic_ = this->declare_parameter<std::string>("ros_to_mqtt.gripper.mqtt", "/fangshe/robot/status/gripper");

        well_ros_topic_ = this->declare_parameter<std::string>("ros_to_mqtt.well.ros", "/fangshe/robot/status/well");
        well_mqtt_topic_ = this->declare_parameter<std::string>("ros_to_mqtt.well.mqtt", "/fangshe/robot/status/well");

        //接收来自mqtt broker的消息
        mqtt_client_ = mosquitto_new(nullptr, true, this);
        if (!mqtt_client_)
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to create MQTT client");
            return;
        }

        // on_mqtt_message处理来自mqtt broker(ui)的消息， on_mqtt_message -> handle_mqtt_message(payload)
        mosquitto_message_callback_set(mqtt_client_, on_mqtt_message);
        // on_connect -> resubscribe_topics -> mosquitto_subscribe，最终订阅话题获取来自mqtt broker(ui)的消息
        mosquitto_connect_callback_set(mqtt_client_, on_connect);
        mosquitto_disconnect_callback_set(mqtt_client_, on_disconnect);

        // 启动连接线程
        connect_thread_ = std::thread(&SeatMqttBridge::connect_loop, this);

        // 启动MQTT循环线程
        int rc = mosquitto_loop_start(mqtt_client_);
        if (rc != 0)
        {
            RCLCPP_ERROR(this->get_logger(), "Could not start MQTT loop: %s", mosquitto_strerror(rc));
            mosquitto_disconnect(mqtt_client_);
            mosquitto_destroy(mqtt_client_);
            return;
        }


        // 创建ROS订阅者（接收ROS消息并转发到MQTT）
        system_sub_ = this->create_subscription<std_msgs::msg::String>(
            system_ros_topic_, 10,
            [this](const std_msgs::msg::String::SharedPtr msg)
            { ros_message_callback(msg, system_mqtt_topic_); });

        pose_sub_ = this->create_subscription<std_msgs::msg::String>(
            pose_ros_topic_, 10,
            [this](const std_msgs::msg::String::SharedPtr msg)
            { ros_message_callback(msg, pose_mqtt_topic_); });

        task_sub_ = this->create_subscription<std_msgs::msg::String>(
            task_ros_topic_, 10,
            [this](const std_msgs::msg::String::SharedPtr msg)
            { ros_message_callback(msg, task_mqtt_topic_); });

        gripper_sub_ = this->create_subscription<std_msgs::msg::String>(
            gripper_ros_topic_, 10,
            [this](const std_msgs::msg::String::SharedPtr msg)
            { ros_message_callback(msg, gripper_mqtt_topic_); });

        well_sub_ = this->create_subscription<std_msgs::msg::String>(
            well_ros_topic_, 10,
            [this](const std_msgs::msg::String::SharedPtr msg)
            { ros_message_callback(msg, well_mqtt_topic_); });


        // 创建ROS发布者（将MQTT消息转发到ROS）
        command_pub_ = this->create_publisher<std_msgs::msg::String>(command_ros_topic_, 10);
        stop_pub_ = this->create_publisher<std_msgs::msg::String>(stop_ros_topic_, 10);

    }

    ~SeatMqttBridge()
    {
        // 停止连接线程
        stop_connect_thread_ = true;
        if (connect_thread_.joinable())
        {
            connect_thread_.join();
        }

        if (mqtt_client_)
        {
            mosquitto_loop_stop(mqtt_client_, true);
            mosquitto_disconnect(mqtt_client_);
            mosquitto_destroy(mqtt_client_);
        }
        mosquitto_lib_cleanup();
    }

    void handle_mqtt_message(const std::string &payload,
                             rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub)
    {

        // 检查是否为空消息
        if (payload.empty()) {
            RCLCPP_WARN(this->get_logger(), "Received empty payload, skipping publication");
            return;
        }

        Json::Reader reader;
        Json::Value root;
        if (!reader.parse(payload, root))
        {
            RCLCPP_WARN(this->get_logger(), "Invalid JSON from MQTT: %s", payload.c_str());
            return;
        }

        auto msg = std_msgs::msg::String();
        msg.data = payload;
        pub->publish(msg);

        RCLCPP_INFO(this->get_logger(), "Published MQTT data to ROS: %s", payload.c_str());

    }

private:
    void ros_message_callback(const std_msgs::msg::String::SharedPtr msg,
                              const std::string &mqtt_topic)
    {
        RCLCPP_INFO(this->get_logger(), "Received ROS message on [%s]: %s",
                    mqtt_topic.c_str(), msg->data.c_str());

        if (!is_connected_)
        {
            RCLCPP_WARN(this->get_logger(), "Not connected to MQTT broker, cannot publish message");
            return;
        }

        int rc = mosquitto_publish(
            mqtt_client_,
            nullptr,
            mqtt_topic.c_str(), // 使用对应的 MQTT topic
            msg->data.length(),
            msg->data.c_str(),
            0,
            false);

        if (rc != 0)
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to publish to MQTT topic [%s]: %s",
                         mqtt_topic.c_str(), mosquitto_strerror(rc));
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "Published ROS data to MQTT topic [%s]: %s",
                        mqtt_topic.c_str(), msg->data.c_str());
        }
    }

    // 连接成功回调，并且有提示[mqtt_bridge-1] [INFO] [1757263328.999195367] [mqtt_bridge]: Successfully connected to MQTT broker
    static void on_connect(struct mosquitto *mosq, void *obj, int rc)
    {
        if (!obj)
            return;

        SeatMqttBridge *bridge = static_cast<SeatMqttBridge *>(obj);

        // 处理连接成功的情况
        if (rc == 0)
        {
            bridge->is_connected_ = true;
            // 成功时输出 [mqtt_bridge-1] [INFO] [1760166425.100243949] [mqtt_bridge]: Successfully connected to MQTT broker
            RCLCPP_INFO(bridge->get_logger(), "Successfully connected to MQTT broker");
            // 连接成功后订阅主题
            bridge->resubscribe_topics();
        }
        else
        {
            bridge->is_connected_ = false;
            RCLCPP_ERROR(bridge->get_logger(), "Failed to connect to MQTT broker: %s", mosquitto_strerror(rc));
        }
    }

    // 断开连接回调
    static void on_disconnect(struct mosquitto *mosq, void *obj, int rc)
    {
        if (!obj)
            return;

        SeatMqttBridge *bridge = static_cast<SeatMqttBridge *>(obj);

        if (rc != 0)
        { // 非自愿断开连接
            bridge->is_connected_ = false;
            RCLCPP_WARN(bridge->get_logger(), "Disconnected from MQTT broker. Will attempt to reconnect...");
        }
    }

    // MQTT消息回调函数（静态方法）
    // 接收broker发送的消息，转换成string，交给handle_mqtt_message(payload)处理
    static void on_mqtt_message(struct mosquitto *mosq, void *obj, const struct mosquitto_message *msg)
    {

        if (!obj || !msg || !msg->payload || !msg->topic)
        {
            return;
        }
        
        if(msg->topic[0] == '\0')
        {
            return;
        }

        SeatMqttBridge *bridge = static_cast<SeatMqttBridge *>(obj);
        std::string payload(static_cast<char *>(msg->payload), msg->payloadlen);

        RCLCPP_INFO(
            bridge->get_logger(),
            "Received MQTT message on topic [%s]: %s",
            msg->topic,
            payload.c_str());

        if (std::string(msg->topic) == bridge->command_mqtt_topic_)
        {
            bridge->handle_mqtt_message(payload, bridge->command_pub_);
        }
        else if (std::string(msg->topic) == bridge->stop_mqtt_topic_)
        {
            bridge->handle_mqtt_message(payload, bridge->stop_pub_);
        }
           
    }

    // 连接循环，处理重连逻辑
    void connect_loop()
    {
        int retry_count = 0;
        const int max_retry_delay = 5; // 最大重连延迟（秒）

        while (rclcpp::ok() && !stop_connect_thread_)
        {
            if (!is_connected_)
            {
                RCLCPP_INFO(this->get_logger(), "Trying to connect to MQTT broker: %s:%d",
                            mqtt_host_.c_str(), mqtt_port_);

                int rc = mosquitto_connect(mqtt_client_, mqtt_host_.c_str(), mqtt_port_, 60);
                if (rc != 0)
                {
                    int delay = std::min(1 << retry_count, max_retry_delay); // 指数退避
                    RCLCPP_ERROR(this->get_logger(), "Could not connect to MQTT broker: %s. Retrying in %d seconds...",
                                 mosquitto_strerror(rc), delay);

                    // 等待重试
                    std::this_thread::sleep_for(std::chrono::seconds(delay));
                    retry_count++;
                }
                else
                {
                    // 连接请求已发送，等待回调确认
                    std::this_thread::sleep_for(std::chrono::seconds(1));
                }
            }
            else
            {
                // 已连接，短暂休眠
                std::this_thread::sleep_for(std::chrono::seconds(1));
            }
        }
    }
    // 订阅主题，目的是获得来自mqtt的消息
    void resubscribe_topics()
    {
        std::vector<std::string> topics = {command_mqtt_topic_, stop_mqtt_topic_};

        for (const auto &topic : topics)
        {
            int rc = mosquitto_subscribe(mqtt_client_, nullptr, topic.c_str(), 0);
            // 处理失败的情况
            if (rc != 0)
            {
                RCLCPP_ERROR(this->get_logger(),
                             "Could not subscribe to MQTT topic [%s]: %s",
                             topic.c_str(),
                             mosquitto_strerror(rc));
            }
            else
            {
                //成功时会输出
                // [mqtt_bridge-1] [INFO] [1760166425.100446656] [mqtt_bridge]: Re-subscribed to MQTT topic: /fangshe/robot/control/command
                // [mqtt_bridge-1] [INFO] [1760166425.100503239] [mqtt_bridge]: Re-subscribed to MQTT topic: /fangshe/robot/control/stop
                RCLCPP_INFO(this->get_logger(),
                            "Re-subscribed to MQTT topic: %s",
                            topic.c_str());
            }
        }
    }
    struct mosquitto *mqtt_client_;
    std::string command_mqtt_topic_;
    std::string stop_mqtt_topic_;
    std::string command_ros_topic_;
    std::string stop_ros_topic_;

    std::string system_ros_topic_;
    std::string system_mqtt_topic_;
    std::string pose_ros_topic_;
    std::string pose_mqtt_topic_;
    std::string task_ros_topic_;
    std::string task_mqtt_topic_;
    std::string gripper_ros_topic_;
    std::string gripper_mqtt_topic_;
    std::string well_ros_topic_;
    std::string well_mqtt_topic_;

    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr stop_pub_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr command_pub_;

    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr system_sub_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr pose_sub_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr task_sub_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr gripper_sub_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr well_sub_;

    JsonData_convert msg_rec;
    JsonData_convert msg_send;
    int ros2_send;
    int count = 0;
    bool is_connected_;
    std::thread connect_thread_;
    bool stop_connect_thread_ = false;
    std::string mqtt_host_;
    int mqtt_port_;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<SeatMqttBridge>());
    rclcpp::shutdown();
    return 0;
}
